#include <ros/ros.h>
#include <sensor_msgs/Imu.h>
#include <fstream>
#include <jsoncpp/json/json.h>
#include <ros/package.h>
#include <Eigen/Geometry>

ros::Publisher imu2_pub;
double roll, pitch, yaw, x, y, z;

bool readExtrinsicParameters() {
    std::string package_path = ros::package::getPath("livox_ros_driver2");
    std::string config_file = package_path + "/config/MID360_config.json";
    
    std::ifstream file(config_file);
    if (!file.is_open()) {
        ROS_ERROR("Cannot open config file: %s", config_file.c_str());
        return false;
    }

    Json::Value root;
    Json::Reader reader;
    if (!reader.parse(file, root)) {
        ROS_ERROR("Failed to parse JSON file");
        return false;
    }

    // 读取第一个雷达的外参
    if (root["lidar_configs"].size() > 0) {
        Json::Value extrinsic = root["lidar_configs"][0]["extrinsic_parameter"];
        roll = extrinsic["roll"].asDouble();
        pitch = extrinsic["pitch"].asDouble();
        yaw = extrinsic["yaw"].asDouble();
        x = extrinsic["x"].asDouble();
        y = extrinsic["y"].asDouble();
        z = extrinsic["z"].asDouble();
        return true;
    }
    return false;
}

void imuCallback(const sensor_msgs::Imu::ConstPtr& imu_msg)
{
    sensor_msgs::Imu imu2_msg;
    imu2_msg.header = imu_msg->header;

    // 创建旋转矩阵
    Eigen::AngleAxisd rollAngle(roll * M_PI / 180.0, Eigen::Vector3d::UnitX());
    Eigen::AngleAxisd pitchAngle(pitch * M_PI / 180.0, Eigen::Vector3d::UnitY());
    Eigen::AngleAxisd yawAngle(yaw * M_PI / 180.0, Eigen::Vector3d::UnitZ());
    Eigen::Quaterniond rotation = yawAngle * pitchAngle * rollAngle;

    // 转换线性加速度
    Eigen::Vector3d acc(imu_msg->linear_acceleration.x,
                       imu_msg->linear_acceleration.y,
                       imu_msg->linear_acceleration.z);
    Eigen::Vector3d rotated_acc = rotation * acc;
    
    imu2_msg.linear_acceleration.x = rotated_acc.x();
    imu2_msg.linear_acceleration.y = rotated_acc.y();
    imu2_msg.linear_acceleration.z = rotated_acc.z();

    // 转换角速度
    Eigen::Vector3d angular(imu_msg->angular_velocity.x,
                          imu_msg->angular_velocity.y,
                          imu_msg->angular_velocity.z);
    Eigen::Vector3d rotated_angular = rotation * angular;
    
    imu2_msg.angular_velocity.x = rotated_angular.x();
    imu2_msg.angular_velocity.y = rotated_angular.y();
    imu2_msg.angular_velocity.z = rotated_angular.z();

    // 转换四元数方向
    Eigen::Quaterniond q_orig(imu_msg->orientation.w,
                             imu_msg->orientation.x,
                             imu_msg->orientation.y,
                             imu_msg->orientation.z);
    Eigen::Quaterniond q_rotated = rotation * q_orig;
    
    imu2_msg.orientation.w = q_rotated.w();
    imu2_msg.orientation.x = q_rotated.x();
    imu2_msg.orientation.y = q_rotated.y();
    imu2_msg.orientation.z = q_rotated.z();

    imu2_pub.publish(imu2_msg);
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, "imu_subscriber_publisher");
    ros::NodeHandle nh;

    if (!readExtrinsicParameters()) {
        ROS_ERROR("Failed to read extrinsic parameters");
        return -1;
    }
    
    ros::Subscriber imu_sub = nh.subscribe("/livox/imu", 5, imuCallback);
    imu2_pub = nh.advertise<sensor_msgs::Imu>("/livox/imu2", 5);

    ros::spin();
    return 0;
}
